//#include "main.h"
#include "modbus_tcp.h"
#include "usr_protocol.h"
#include "error_code.h"
#include "firmware_update.h"


#define TCPBUFLEN  125

uint8_t  rtubuf1[MBLENGTH1/8];
uint16_t rtubuf3[MBLENGTH3];



#define MODBUS_FUNC_ENTER_BOOT   0x50  // 进入bootloader
#define MODBUS_FUNC_WRITE_FLASH  0x51  // 写入数据(1KB一块)
#define MODBUS_FUNC_VERIFY_CRC   0x52  // 校验CRC并重启


uint8_t TCPMDSaveData(uint16_t add,uint8_t* p_val,uint16_t length_word,uint8_t channel)	   //����6��16�ص�����
{		
		uint8_t i = 0;
		if(add<MAX_MOTOR_REG)
		{
			for(i = 0;i<length_word;i++)
			{
				motor_status_reg_map[channel][add+i] = (((*(p_val+2*i))&0x00ff)<<8) + ((*(p_val+2*i+1))&0x00ff);
			}				
			return 1;
		}
        else if (add < (MAX_BOARD_PARAM_REG + 2000))
        {
			
			add = add - 2000;
            for(i = 0;i<length_word;i++)
            {
                control_board_param_reg_map[channel][add+i] =(((*(p_val+2*i))&0x00ff)<<8) + ((*(p_val+2*i+1))&0x00ff);
            }
            return 1;
        }
        else 
        {
            
        }

		return 0;
}

int16_t TCPMDReadData(uint16_t add,uint8_t channel)	   //����6��16�ص�����
{		
		uint8_t i = 0;
		if(add<MAX_MOTOR_REG)
		{

			return motor_status_reg_map[channel][add];
		}
        else if (add < (MAX_BOARD_PARAM_REG + 2000))
        {
			add  = add - 2000;
            return control_board_param_reg_map[channel][add+i];

        }
        else 
        {
            
        }

		return 0;
}



/*--------------------------------------------------------------------------------------
�����־  Э���ʶ  ����   ��Ԫ��ʶ  ������   ����
2Byte     2Byte     2Byte   1Byte    1Byte    Nbyte
���ص� �ǳ���
----------------------------------------------------------------------------------------*/
uint16_t tcp_modslv(uint8_t * rbuf,uint8_t *sbuf,uint8_t channle) //���س���
{
	uint16_t ii;
	uint16_t j;
	uint16_t start_id_addr,end_id_addr;
	uint16_t mbstaadd,mblength; 
	uint16_t t01,tt0,i01;
    uint16_t RBack_Lenth=0;
    uint16_t item =0;
	
	for(ii=0;ii<4;ii++)
	{
		*(sbuf+ii) = *(rbuf+ii);		
	}
//	if(*(rbuf+5) != 6) //����
//	{
//			return 0;
//	}
	
	*(sbuf+7) = *(rbuf+7); //������
	
	switch(*(sbuf+7))
	{

			case 1:	//����Ȧ ������	
				
				mbstaadd = *(rbuf+8);
				mbstaadd <<= 8;
				mbstaadd += *(rbuf+9);

				mblength = *(rbuf+10);
				mblength <<= 8;
				mblength += *(rbuf+11);		
		
				if((mbstaadd+mblength)>(MBSTAADD1+MBLENGTH1))
				{
						return 0; 
				}
				
		    tt0 = mblength+8;	
				t01 = 0;
				while(tt0>8)
				{
					*(sbuf+9+t01) = 0;
					tt0 -= 8;
					t01++;
				}	

				
        *(sbuf+5) = 3+((mblength+7)/8);				
 				*(sbuf+6) = *(rbuf+6);	//��ַ
				*(sbuf+8) = ((mblength+7)/8);
				
				if(*(sbuf+8)>0xff)
				{
					 return 0;
				}
				
				for(ii = 0;ii<mblength;ii++)
				{
					i01 = mbstaadd + ii-MBSTAADD1;
					tt0 = (uint8_t)(i01>>3);
					t01 = (uint8_t)(i01&7);

					if(ii>TCPBUFLEN)
					{
						  return 0;
					}
					
					if((rtubuf1[tt0]&(1<<t01))!=0)
					{
						tt0 = (uint8_t)(ii>>3);
						t01 = (uint8_t)(ii&7);
						*(sbuf+9+tt0) |= (1<<t01);					   
					}
				}
		    RBack_Lenth = 6+*(sbuf+5);
		    break;
				
			case 3:// ������Ĵ��� ������
				mbstaadd = *(rbuf+8);
				mbstaadd <<= 8;
				mbstaadd += *(rbuf+9);
				mblength = *(rbuf+10);
				mblength <<= 8;
				mblength += *(rbuf+11);
			
//				if((mbstaadd+mblength)>(MBSTAADD3+MBLENGTH3))
//				{
//						return 0; 
//				}				
        *(sbuf+5)  = 3+(mblength*2);
				*(sbuf+6) = *(rbuf+6);	//��ַ
				*(sbuf+8) = 2*mblength;	//���س���	
				for(ii = 0;ii<mblength;ii++)
				{
					
						if(ii>=TCPBUFLEN)
						{
						   return 0;
						}
						tt0 = mbstaadd+ii-MBSTAADD3 ;
                        item  = TCPMDReadData(tt0,channle);
						*(sbuf+9+ii+ii) = (uint8_t)(item>>8);
						*(sbuf+10+ii+ii) = (uint8_t)(item)&0x00ff;														
				}				
				RBack_Lenth = (6+*(sbuf+5));
			  break;
			case 0x20:// ������Ĵ��� ������
				mbstaadd = *(rbuf+8);
				mbstaadd <<= 8;
				mbstaadd += *(rbuf+9);
				mblength = *(rbuf+10);
				mblength <<= 8;
				mblength += *(rbuf+11);
			
//				if((mbstaadd+mblength)>(MBSTAADD3+MBLENGTH3))
//				{
//						return 0; 
//				}				
        *(sbuf+5)  = 3+(mblength*2);
				*(sbuf+6) = *(rbuf+6);	//��ַ
				//*(sbuf+8) = 2*mblength;	//���س���	
				*(sbuf+8) = *(rbuf+8);	//���ؼĴ�����ַ	 //�Ա�׼Э�� �����˸Ķ� ȡ���˳��ȣ�����һ���ֽڣ������Ĵ�����ַ
        *(sbuf+9) = *(rbuf+9);	//���ؼĴ�����ַ	 //�Ա�׼Э�� �����˸Ķ�
				for(ii = 0;ii<mblength;ii++)
				{
					
						if(ii>=TCPBUFLEN)
						{
						   return 0;
						}
						tt0 = mbstaadd+ii-MBSTAADD3 ;
                        item  = TCPMDReadData(tt0,channle);
						*(sbuf+10+ii+ii) = (uint8_t)(item>>8);
						*(sbuf+11+ii+ii) = (uint8_t)(item)&0x00ff;														
				}				
				RBack_Lenth = (6+*(sbuf+5));
			  break;
			case 5:
				
			break;		
				
			case 06://д�����Ĵ��� ������
					
				mbstaadd = *(rbuf+8);
				mbstaadd <<= 8;
				mbstaadd += *(rbuf+9);// �Ĵ�����ַ

				if(mbstaadd > 2000+MAX_BOARD_PARAM_REG)
				{
					*comm_error[channle] |= COMM_ERR_ADDR;
					return 0;
				}
//				g_CmdMap[mbstaadd] = ; 
//				tt0 = mbstaadd-MBSTAADD3 ;
//				t01 =(u16)(*(rbuf+10));
//				t01 <<= 8;
//				t01 += (u16)(*(rbuf+11));				
				TCPMDSaveData(mbstaadd,(uint8_t *) (rbuf+10),1,channle);


				if(mbstaadd<= (MOTOR_SETPOINT_POSTION_BASE + MAX_ID) && (mbstaadd+mblength)>=MOTOR_SETPOINT_POSTION_BASE)
				{
					start_id_addr = mbstaadd-MOTOR_SETPOINT_POSTION_BASE;
                    setFlag(motor_position_flags, start_id_addr);
				}

                send_modbus_command(mbstaadd,((rbuf[10]<<8)|rbuf[11] ),channle);
				*(sbuf+4) = *(rbuf+4);
				*(sbuf+5) = *(rbuf+5);	
				*(sbuf+6) = *(rbuf+6);		
				*(sbuf+8)  = *(rbuf+8);
				*(sbuf+9)  = *(rbuf+9);
				*(sbuf+10) = *(rbuf+10);
				*(sbuf+11) = *(rbuf+11);						
				RBack_Lenth = (6+*(sbuf+5));
									
				break;				
		
			case 0x10://д����Ĵ��� ������
					
				mbstaadd = *(rbuf+8);
				mbstaadd <<= 8;
				mbstaadd += *(rbuf+9);
				mblength = *(rbuf+10);
				mblength <<= 8;
				mblength += *(rbuf+11);
				if(mbstaadd > (2000+MAX_BOARD_PARAM_REG)||(mbstaadd+mblength) > (2000+MAX_BOARD_PARAM_REG))
				{
					*comm_error[channle] |= COMM_ERR_ADDR;
					return 0;
				}
				TCPMDSaveData(mbstaadd,(uint8_t *) (rbuf+13),mblength,channle);

				if(mbstaadd<= (MOTOR_SETPOINT_POSTION_BASE + MAX_ID) && (mbstaadd+mblength)>=MOTOR_SETPOINT_POSTION_BASE)
				{
					start_id_addr = mbstaadd-MOTOR_SETPOINT_POSTION_BASE;
					end_id_addr = mbstaadd+mblength - MOTOR_SETPOINT_POSTION_BASE;
					for(j=start_id_addr;j<=end_id_addr;j++)
					{
						// 使用位操作设置标志
                        setFlag(motor_position_flags, j);
					}
				}
				send_modbus_command(mbstaadd,0,channle);
				*(sbuf+4) = *(rbuf+4);
				*(sbuf+5) = *(rbuf+5);	
				*(sbuf+6) = *(rbuf+6);		
				*(sbuf+8)  = *(rbuf+8);
				*(sbuf+9)  = *(rbuf+9);
				*(sbuf+10) = *(rbuf+10);
				*(sbuf+11) = *(rbuf+11);						

				RBack_Lenth = (6+*(sbuf+5));
									
				break;		

			case MODBUS_FUNC_ENTER_BOOT:
			{

				// 设置升级标志并重启
				BootParam_t param = {
					.magic = BOOT_MAGIC,
					.boot_flag = BOOT_FLAG_UPGRADE,
					.fw_size = 0,
					.fw_crc = 0
				};
				write_boot_param(&param);
				NVIC_SystemReset();
				
				break;
			}
			case MODBUS_FUNC_WRITE_FLASH:
			{
				// 数据格式:
				// rbuf[8]: 块号(0-59)
				// rbuf[9-1032]: 1KB数据
				uint8_t block = rbuf[8];
				uint8_t status = handle_boot_write(block, &rbuf[9]);
				
				*(sbuf+5) = 3;  // 返回长度
				*(sbuf+8) = status;
				RBack_Lenth = 9;
				break;
			}
			
			case MODBUS_FUNC_VERIFY_CRC:
			{
				uint32_t crc = (rbuf[8] << 24) | (rbuf[9] << 16) | 
								(rbuf[10] << 8) | rbuf[11];
				uint8_t status = handle_boot_verify(crc);
				
				*(sbuf+5) = 3;
				*(sbuf+8) = status;
				RBack_Lenth = 9;
				break;
			}
			
		
		  default:
			 RBack_Lenth = 0;
			 break;
  }
	
return RBack_Lenth; 	
}





